// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PathPlanningAndTrackingMetricsControllerTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Core.UnitTests
{
    using System;
    using System.Text;
    using Microsoft.Robotics.FiniteStateMachine;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Navigation;
    using Test.Robotics.Navigation.Metrics.PathPlanningAndTracking;

    /// <summary>
    /// Tests for path planning/tracking metrics controller state machine.
    /// </summary>
    [TestClass]
    public class PathPlanningAndTrackingMetricsControllerTests
    {
        /// <summary>
        /// Charge to 99% once battery is low.
        /// </summary>
        ////[TestMethod]
        ////[TestCategory("Unit")]
        public void PathPlanningAndTrackingMetricsControllerNeedToChargeTest()
        {
            var controller = this.Controller();
            Assert.AreEqual("State:Initial", controller.ToString());

            controller.UpdateBattery(true, 30, DateTime.MinValue);
            Assert.AreEqual("State:Charging", controller.ToString());

            controller.UpdateBattery(true, 40, DateTime.MinValue);
            Assert.AreEqual("State:Charging", controller.ToString());

            controller.UpdateBattery(true, 50, DateTime.MinValue);
            Assert.AreEqual("State:Charging", controller.ToString());

            controller.UpdateBattery(true, 99, DateTime.MinValue);
            Assert.AreEqual("State:Initial", controller.ToString());

            var output = controller.UpdateBattery(true, 99, DateTime.MinValue);
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-1, output.WaypointId); // special undocking value
        }

        /// <summary>
        /// Begin without charging if not below 95% already.
        /// </summary>
        ////[TestMethod]
        ////[TestCategory("Unit")]
        public void PathPlanningAndTrackingMetricsControllerChargedTest()
        {
            var controller = this.Controller();
            Assert.AreEqual("State:Initial", controller.ToString());

            var output = controller.UpdateBattery(true, 95, DateTime.MinValue);
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-1, output.WaypointId); // special undocking value
        }

        /// <summary>
        /// Undock and begin tracking once charged.
        /// </summary>
        ////[TestMethod]
        ////[TestCategory("Unit")]
        public void PathPlanningAndTrackingMetricsControllerUndockingTest()
        {
            var controller = this.Controller();
            Assert.AreEqual("State:Initial", controller.ToString());

            var output = controller.UpdateBattery(true, 95, DateTime.MinValue);
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-1, output.WaypointId); // special undocking value

            controller.UpdateBattery(false, 95, DateTime.MinValue.AddSeconds(1));
            Assert.AreEqual("State:Undocking", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(7));
            Assert.AreEqual("State:Orienting", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Tracking", controller.ToString());
        }

        /// <summary>
        /// Track waypoints.
        /// </summary>
        ////[TestMethod]
        ////[TestCategory("Unit")]
        public void PathPlanningAndTrackingMetricsControllerTrackingTest()
        {
            var controller = this.Controller();
            Assert.AreEqual("State:Initial", controller.ToString());

            var output = controller.UpdateBattery(true, 95, DateTime.MinValue);
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-1, output.WaypointId); // special undocking value

            controller.UpdateBattery(false, 95, DateTime.MinValue.AddSeconds(7));
            Assert.AreEqual("State:Orienting", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Tracking", controller.ToString());

            output = controller.UpdateDrive(new DriveDirection(3.14, 0.5, 1), DateTime.MinValue);
            Assert.AreEqual("State:Tracking", controller.ToString());
            Assert.AreEqual(3.14, output.Drive.Heading);
            Assert.AreEqual(0.5, output.Drive.Forward);

            this.SimulatePose(new Pose2D(50, 50, 0), controller);
            Assert.AreEqual("State:Tracking", controller.ToString());

            output = controller.UpdateDrive(new DriveDirection(2, 0.2, 1), DateTime.MinValue);
            Assert.AreEqual("State:Tracking", controller.ToString());
            Assert.AreEqual(2, output.Drive.Heading);
            Assert.AreEqual(0.2, output.Drive.Forward);

            this.SimulatePose(new Pose2D(101, 101, 0), controller);
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            output = this.SimulatePose(new Pose2D(100, 100, 0), controller);
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Tracking", controller.ToString());
            Assert.AreEqual("Tracking - Next Waypoint (1)", output.Message);
            Assert.AreEqual(200, output.Target.Value.X);
            Assert.AreEqual(200, output.Target.Value.Y);
            Assert.AreEqual(double.NaN, output.Target.Value.Heading);

            output = controller.UpdateDrive(new DriveDirection(3, 0.7, 1), DateTime.MinValue);
            Assert.AreEqual("State:Tracking", controller.ToString());
            Assert.AreEqual(3, output.Drive.Heading);
            Assert.AreEqual(0.7, output.Drive.Forward);

            output = this.SimulatePose(new Pose2D(150, 150, 0), controller);
            Assert.AreEqual("State:Tracking", controller.ToString());
            Assert.AreEqual(3, output.Drive.Heading);
            Assert.AreEqual(0.7, output.Drive.Forward);

            output = controller.UpdateDrive(new DriveDirection(4, -0.1, 1), DateTime.MinValue);
            Assert.AreEqual("State:Tracking", controller.ToString());
            Assert.AreEqual(4, output.Drive.Heading);
            Assert.AreEqual(-0.1, output.Drive.Forward);

            this.SimulatePose(new Pose2D(201, 201, 0), controller);
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            output = this.SimulatePose(new Pose2D(200, 200, 0), controller);
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(10, 20, 30)); // not in agreement
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(10, 20, 30));
            Assert.AreEqual("State:Tracking", controller.ToString());
            Assert.AreEqual("Tracking - Next Waypoint (2)", output.Message);
            Assert.AreEqual(0, output.Target.Value.X);
            Assert.AreEqual(0, output.Target.Value.Y);
            Assert.AreEqual(0, output.Target.Value.Heading);
        }

        /// <summary>
        /// Abandon tracking after long timeout.
        /// </summary>
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(AssertFailedException))]
        public void PathPlanningAndTrackingMetricsControllerAbandonTest()
        {
            var controller = this.Controller();
            Assert.AreEqual("State:Initial", controller.ToString());

            var output = controller.UpdateBattery(true, 95, DateTime.MinValue);
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-1, output.WaypointId); // special undocking value

            controller.UpdateBattery(false, 95, DateTime.MinValue.AddSeconds(7));
            Assert.AreEqual("State:Tracking", controller.ToString());

            controller.UpdateDrive(new DriveDirection(3.14, 0.5, 1), DateTime.MinValue.AddHours(1));
            Assert.AreEqual("State:Aborted", controller.ToString());

            // this will Assert.Fail due to abandonment followed by charging
            controller.UpdateBattery(true, 50, DateTime.MinValue);
        }

        /// <summary>
        /// Pre-Docking test.
        /// </summary>
        ////[TestMethod]
        ////[TestCategory("Unit")]
        public void PathPlanningAndTrackingMetricsControllerPredockingTest()
        {
            var controller = this.Controller();
            Assert.AreEqual("State:Initial", controller.ToString());

            var output = controller.UpdateBattery(true, 95, DateTime.MinValue);
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-1, output.WaypointId); // special undocking value

            controller.UpdateBattery(false, 95, DateTime.MinValue.AddSeconds(7));
            Assert.AreEqual("State:Orienting", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(101, 101, 0), controller); // 1st waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(100, 100, 0), controller);
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(201, 201, 0), controller); // 2nd waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(200, 200, 0), controller);
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(1, 1, 0), controller); // 3rd waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            output = this.SimulatePose(new Pose2D(0, 0, 0), controller);
            Assert.AreEqual("State:Measuring", controller.ToString());
            Assert.IsFalse(output.RecordNavMetrics);

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            output = controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateBattery(false, 20, DateTime.MinValue.AddSeconds(5));
            output = controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Docking", controller.ToString());
            Assert.IsTrue(output.RecordNavMetrics);
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-2, output.WaypointId); // special docking value
        }

        /// <summary>
        /// Docking not needed test (battery still charged sufficiently).
        /// </summary>
        ////[TestMethod]
        ////[TestCategory("Unit")]
        public void PathPlanningAndTrackingMetricsControllerDockingNotNeededTest()
        {
            var controller = this.Controller();
            Assert.AreEqual("State:Initial", controller.ToString());

            var output = controller.UpdateBattery(true, 95, DateTime.MinValue);
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-1, output.WaypointId); // special undocking value

            controller.UpdateBattery(false, 95, DateTime.MinValue.AddSeconds(7));
            Assert.AreEqual("State:Orienting", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(101, 101, 0), controller); // 1st waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(100, 100, 0), controller); // 1st waypoint
            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            controller.UpdateTime(DateTime.MinValue.AddSeconds(10));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(201, 201, 0), controller); // 2nd waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(200, 200, 0), controller); // 1st waypoint
            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            controller.UpdateTime(DateTime.MinValue.AddSeconds(10));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(1, 1, 0), controller); // 3rd waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(0, 0, 0), controller); // 1st waypoint
            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            output = controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());
            Assert.IsFalse(output.RecordNavMetrics);

            controller.UpdateTime(DateTime.MinValue.AddSeconds(10));
            output = controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Docking", controller.ToString());
            Assert.IsTrue(output.RecordNavMetrics);
            Assert.IsTrue(output.RecordLocMetrics);
        }

        /// <summary>
        /// Successful docking test.
        /// </summary>
        ////[TestMethod]
        ////[TestCategory("Unit")]
        public void PathPlanningAndTrackingMetricsControllerSuccessfulDockingTest()
        {
            var controller = this.Controller();
            Assert.AreEqual("State:Initial", controller.ToString());

            var output = controller.UpdateBattery(true, 95, DateTime.MinValue);
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-1, output.WaypointId); // special undocking value

            controller.UpdateBattery(false, 95, DateTime.MinValue.AddSeconds(7));
            Assert.AreEqual("State:Orienting", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(101, 101, 0), controller); // 1st waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(100, 100, 0), controller); // 1st waypoint
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            controller.UpdateTime(DateTime.MinValue.AddSeconds(10));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(201, 201, 0), controller); // 2nd waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(200, 200, 0), controller);
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            controller.UpdateTime(DateTime.MinValue.AddSeconds(10));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(1, 1, 0), controller); // 3rd waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            output = this.SimulatePose(new Pose2D(0, 0, 0), controller);
            Assert.AreEqual("State:Measuring", controller.ToString());
            Assert.IsFalse(output.RecordNavMetrics);

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateBattery(false, 20, DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Docking", controller.ToString());
            Assert.IsTrue(output.RecordNavMetrics);
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-2, output.WaypointId); // special docking value

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            output = controller.UpdateTime(DateTime.MinValue);
            Assert.AreEqual("State:Docking", controller.ToString());

            // aligning
            output = controller.UpdateTime(DateTime.MinValue.AddSeconds(10));
            Assert.AreEqual("State:Docking", controller.ToString());
            Assert.AreEqual("Docking - Aligning", output.Message);
            Assert.AreEqual(0, output.Drive.Heading);
            Assert.AreEqual(0, output.Drive.Forward);

            // backing - charging
            output = controller.UpdateBattery(true, 50, DateTime.MinValue.AddSeconds(30));
            Assert.AreEqual("State:Complete", controller.ToString());
            Assert.AreEqual("Docking - Success", output.Message);

            // run metrics again!
            output = controller.UpdateTime(DateTime.MinValue.AddSeconds(40));
            Assert.AreEqual("State:Initial", controller.ToString());
            Assert.AreEqual(double.NaN, output.Drive.Heading);
            Assert.AreEqual(0, output.Drive.Forward);
            Assert.AreEqual(100, output.Target.Value.X);
            Assert.AreEqual(100, output.Target.Value.Y);
            Assert.AreEqual(double.NaN, output.Target.Value.Heading);
        }

        /// <summary>
        /// Failed docking test.
        /// </summary>
        ////[TestMethod]
        ////[TestCategory("Unit")]
        public void PathPlanningAndTrackingMetricsControllerFailedDockingTest()
        {
            var controller = this.Controller();
            Assert.AreEqual("State:Initial", controller.ToString());

            var output = controller.UpdateBattery(true, 95, DateTime.MinValue);
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-1, output.WaypointId); // special undocking value

            controller.UpdateBattery(false, 95, DateTime.MinValue.AddSeconds(7));
            Assert.AreEqual("State:Orienting", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(101, 101, 0), controller); // 1st waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(100, 100, 0), controller); // 1st waypoint
            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            controller.UpdateTime(DateTime.MinValue.AddSeconds(10));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(201, 201, 0), controller); // 2nd waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(200, 200, 0), controller);
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(1, 1, 0), controller); // 2nd waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(0, 0, 0), controller); // 3rd waypoint
            Assert.AreEqual("State:Measuring", controller.ToString());

            output = controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Measuring", controller.ToString());
            Assert.IsFalse(output.RecordNavMetrics);

            controller.UpdateBattery(false, 20, DateTime.MinValue.AddSeconds(5));
            output = controller.UpdateLocMetrics(new Pose2D(1, 2, 3));
            Assert.AreEqual("State:Docking", controller.ToString());
            Assert.IsTrue(output.RecordNavMetrics);
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-2, output.WaypointId); // special docking value

            controller.UpdateTime(DateTime.MinValue);
            Assert.AreEqual("State:Docking", controller.ToString());

            // aligning
            output = controller.UpdateTime(DateTime.MinValue.AddSeconds(10));
            Assert.AreEqual("State:Docking", controller.ToString());
            Assert.AreEqual("Docking - Aligning", output.Message);
            Assert.AreEqual(0, output.Drive.Heading);
            Assert.AreEqual(0, output.Drive.Forward);

            // backing - *not* charging
            output = controller.UpdateBattery(false, 50, DateTime.MinValue.AddSeconds(30));
            Assert.AreEqual("State:Failed", controller.ToString());
            Assert.AreEqual("Docking - Failure", output.Message);
            Assert.AreEqual(double.NaN, output.Drive.Heading);
            Assert.AreEqual(0, output.Drive.Forward);

            // try again
            output = controller.UpdateBattery(true, 50, DateTime.MinValue.AddSeconds(40));
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.AreEqual(double.NaN, output.Drive.Heading);
            Assert.AreEqual(0.1, output.Drive.Forward);
            Assert.AreEqual(200, output.Target.Value.X);
            Assert.AreEqual(200, output.Target.Value.Y);
            Assert.AreEqual(double.NaN, output.Target.Value.Heading);
        }

        /// <summary>
        /// Ground truth measurement test.
        /// </summary>
        ////[TestMethod]
        ////[TestCategory("Unit")]
        public void PathPlanningAndTrackingMetricsControllerGroundTruthTest()
        {
            var controller = this.Controller();
            Assert.AreEqual("State:Initial", controller.ToString());

            var output = controller.UpdateBattery(true, 95, DateTime.MinValue);
            Assert.AreEqual("State:Undocking", controller.ToString());
            Assert.IsTrue(output.RecordLocMetrics);
            Assert.AreEqual(-1, output.WaypointId); // special undocking value

            controller.UpdateBattery(false, 95, DateTime.MinValue.AddSeconds(7));
            Assert.AreEqual("State:Orienting", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Tracking", controller.ToString());

            this.SimulatePose(new Pose2D(101, 101, 0), controller); // 1st waypoint
            Assert.AreEqual("State:Scanning", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(50));
            Assert.AreEqual("State:Approaching", controller.ToString());

            this.SimulatePose(new Pose2D(100, 100, 0), controller);
            Assert.AreEqual("State:Measuring", controller.ToString());

            this.SimulatePose(new Pose2D(101, 100, 0), controller); // wiggling around
            Assert.AreEqual("State:Measuring", controller.ToString());

            this.SimulatePose(new Pose2D(101, 101, 0), controller); // wiggling around
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateTime(DateTime.MinValue.AddSeconds(5));
            controller.UpdateLocMetrics(new Pose2D(1, 2, 3)); // first sample
            Assert.AreEqual("State:Measuring", controller.ToString());

            controller.UpdateLocMetrics(new Pose2D(1, 2, 3)); // measurement done
            Assert.AreEqual("State:Tracking", controller.ToString());
        }

        /// <summary>
        /// Create a controller instance with test targets, and wired to test methods.
        /// </summary>
        /// <returns>Controller instance.</returns>
        private PathPlanningAndTrackingMetricsController Controller()
        {
            return new PathPlanningAndTrackingMetricsController(
                minimumGroundTruthSamplesPerWaypoint: 1,
                groundTruthSampleConsensusThresholdInMillimeters: 100,
                batteryLowThreshold: 50,
                waypointArrivalThreshold: 0.02,
                waypoints: new Waypoint[]
                {
                    new Waypoint(new Pose2D(100, 100, double.NaN), new Pose2D(double.NaN, double.NaN, double.NaN), true, 0, false),
                    new Waypoint(new Pose2D(200, 200, double.NaN), new Pose2D(double.NaN, double.NaN, double.NaN), true, 1, false),
                    new Waypoint(new Pose2D(0, 0, 0), new Pose2D(double.NaN, double.NaN, double.NaN), true, 2, true)
                },
                measureGroundTruth: true,
                mapResolution: 0.1,
                usingNavBenchmark: false);
        }

        /// <summary>
        /// Simulate metrics reporting a given pose.
        /// </summary>
        /// <param name="pose">Pose to simulate.</param>
        /// <param name="controller">Controller to which to apply pose update.</param>
        /// <returns>Current controller status.</returns>
        private PathPlanningAndTrackingMetricsControllerStatus.StatusOutput SimulatePose(Pose2D pose, PathPlanningAndTrackingMetricsController controller)
        {
            return controller.UpdateNavMetrics(new PathPlanningAndTrackingMetrics(0, 0, TimeSpan.Zero, pose, 0, 0, 0, 0), DateTime.MinValue);
        }
    }
}
